#include "./fridge.h"
#include "../device/gpio.h"
#include "./sensor.h"
#include "./spi_flash.h"

namespace driver {

using namespace device;
using namespace utils;

struct Info {
    uint16_t speed;

    void reset() {
        speed = 200;
    }
};

static store_t<FridgeAddr, Info> info;

using PWM = PA<3>; // TIM9_CH2 <=> AF3
using DIR1 = PE<6>;
using DIR2 = PE<7>;
using COUNTER = PD<12>; // TIM4_CH1 <=> AF2

Fridge::Fridge() {
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN |
        RCC_AHB1ENR_GPIOEEN |
        RCC_AHB1ENR_GPIODEN;
    RCC->APB2ENR |= RCC_APB2ENR_TIM9EN;
    RCC->APB1ENR |= RCC_APB1ENR_TIM4EN;

    ConfigAf<3, PWM>();
    ConfigOutput<DIR1, DIR2>();
    ConfigAf<2, COUNTER>();

	TIM9->CR1 = 0x00;
	TIM9->CR2 = 0x00;
	TIM9->PSC = 168  - 1;
	TIM9->ARR = 100 - 1;
	TIM9->CCR2 = 100;
	TIM9->CCMR1 = 0x6000;
	TIM9->CCER = 0x0010;
	TIM9->CR1 = 0x01;
	TIM9->EGR = 0x01;

	TIM4->CR1 = 0x00;
	TIM4->CR2 = 0x00;
	TIM4->SMCR = 0x57;
	TIM4->CCMR1 = 0xF1;
	TIM4->CCER &= ~0x0A;
	TIM4->CR1 = 0x01;

    set_dir(dir_t::Brake);
	set_param(0.03f, 0.03f, 0.01f);

	info.load();
}

utils::error_t Fridge::open() {
	return open(info->speed);
}

utils::error_t Fridge::close() {
	return close(info->speed);
}

void Fridge::set_target_speed(uint16_t speed) {
	info->speed = speed;
	info.save();
}

uint16_t Fridge::get_target_speed() {
	return info->speed;
}

void Fridge::set_voltage(uint16_t val) {
    TIM9->CCR2 = val;
}

uint16_t Fridge::get_count() {
    return TIM4->CNT;
}

void Fridge::reset_count() {
    TIM4->CNT = 0;
}

void Fridge::set_dir(Fridge::dir_t dir) {
    switch (dir) {
        case dir_t::Open:
            DIR1::Set();
            DIR2::Clr();
        break;

        case dir_t::Close:
            DIR1::Clr();
            DIR2::Set();
        break;
        
        case dir_t::Brake:
            DIR1::Clr();
            DIR2::Clr();
        break;
    }
}

error_t Fridge::open(uint16_t speed) {
	error_t err;
	uint16_t position = 0;
    if (sensor->is_fridge_open()) {
        return err;
    }

	init_ctrl(speed);
	reset_count();
	set_dir(dir_t::Open);
	motor_run();
	for (; !sensor->is_fridge_open();) {
		osDelay(20);
		position = get_count();
		exec(position);
		err = is_ok();
		if (err) {
			break;
		}
	}
	set_dir(dir_t::Brake);
	init_ctrl(0);
	return err;
}

error_t Fridge::close(uint16_t speed) {
	error_t err;
	uint16_t position = 0;
    if (sensor->is_fridge_close()) {
        return err;
    }

	init_ctrl(speed);
	reset_count();
	set_dir(dir_t::Close);
	motor_run();
	for (; !sensor->is_fridge_close();) {
		osDelay(20);
		position = get_count();
		exec(position);
		err = is_ok();
		if (err) {
			break;
		}
	}
	set_dir(dir_t::Brake);
	init_ctrl(0);
	return err;
}

}

